#!/usr/bin/env python
import roslib; roslib.load_manifest('DelphinROSv2')
import roslib
import rospy
import time
from std_msgs.msg import Int8
from library_highlevel import library_highlevel

def main(controller):
    rospy.init_node('back_seat_driver')
    pub=rospy.Publisher('back_seat_flag',Int8)
    
    # Store Initial Time
    time_zero = time.time()    
    
    time.sleep(15) #Allow critical systems to come online.
   
  
    # Import Limit Parameters from laumch file
    overDepth = rospy.get_param('over-depth')
    overPitch = rospy.get_param('over-pitch')    
    overRoll = rospy.get_param('over-roll')     
    maxInternalTemp = rospy.get_param('max-internal-temp')  
    minMotorVoltage = rospy.get_param('min-motor-voltage')
    missionTimeout = rospy.get_param('mission-timeout')
    
    #Initialise BackSeatFlag to zero
    BackSeatFlag=0
    
    
    while not rospy.is_shutdown():
        time_elapsed=time.time()-time_zero
    
    
        #Poll System For Any Potential Errors or Status Warnings
    
        #Identify OverDepth?
        current_depth=controller.getDepth()

        
        if current_depth>overDepth: 
            BackSeatFlag=1
            str = "Current depth %sm > Depth limt of %sm" %(current_depth, overDepth) 
            rospy.logerr(str)
            pub.publish(BackSeatFlag)
            return
        
        #Identify OverPitch?
        current_pitch=controller.getPitch()
        
        if current_pitch>overPitch: 
            BackSeatFlag=1
            str = "Current pitch %sdeg > Pitch limt of %sdeg" %(current_pitch, overPitch) 
            rospy.logerr(str)
            pub.publish(BackSeatFlag)
            return
        
        #Identify OverRoll?
        current_roll=controller.getRoll()
        
        if current_roll>overRoll: 
            BackSeatFlag=1
            str = "Current roll %sdeg > Roll limit of %sdeg" %(current_roll, overRoll) 
            rospy.logerr(str)
            pub.publish(BackSeatFlag)
            return        

        #Check Internal Pressure Vessel Temperature
        current_temperature=controller.getTemperature()
        
        if current_temperature>maxInternalTemp: 
            BackSeatFlag=1
            str = "Current temperature %sdeg > Temperature limit of %sdeg" %(current_temperature, maxInternalTemp) 
            rospy.logerr(str)
            pub.publish(BackSeatFlag)
            return   
        
        #Check Motor Voltage
        current_voltage=controller.getVoltage()
        
        if current_voltage<minMotorVoltage: 
            BackSeatFlag=1
            str = "Current voltage %smV < Motor voltage limit of %smV" %(current_voltage, minMotorVoltage) 
            rospy.logerr(str)
            pub.publish(BackSeatFlag)
            return                 
        
        #Check Mission Duration
        current_time=time.time()-time_zero
        
        if current_time>missionTimeout: 
            BackSeatFlag=1
            str = "Current mission time %ss > Mission time limit of %ss" %(current_time, missionTimeout) 
            rospy.logerr(str)
            pub.publish(BackSeatFlag)
            return   
        
 
if __name__ == '__main__':
    # Define an instance of highlevelcontrollibrary to pass to all action servers
    lib = library_highlevel()
    main(lib)
    rospy.spin()
